How to add matrix diagonal elements in C